#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/factory.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/solvers/csparse/linear_solver_csparse.h"
 
#include "g2o/types/slam3d/types_slam3d.h"
 
#include <iostream>
 
#define MAXITERATION 50

class Calibrator
{
    int sensor_num;
    int transfer_num=0;
    std::map<std::string, int> sensor_map;
    std::vector<std::vector<double>> transfer_vector;

    Eigen::Isometry3d setMeasure(int num);
    void readCfg();

    typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,6> > Block;

    std::vector<g2o::VertexSE3*> vertexSet;
public:
    void Run();
    Calibrator();
};
